#include "io_hardinterface.h"
#include "io_macro.h"
#include <iostream>
#include "rclcpp/rclcpp.hpp"
#include <map>

namespace io_module {

IoHardInterface::IoHardInterface(const std::shared_ptr<rclcpp::Node>& node):
    m_priNh(node), m_io_factory(node), m_init_ok(false)
{
}

IoHardInterface::~IoHardInterface()
{
    m_quit_mutex.unlock();
    m_thread.join();
}

bool IoHardInterface::init()
{
    if (m_init_ok)
    {
        return false;
    }
    m_init_ok = read_param_server();
    m_quit_mutex.lock();
    m_thread = std::thread(thread_fun, this);
    return m_init_ok;
}

// 暂时设置读写互斥
bool IoHardInterface::read_input_ios(const int &count_io, uint64_t &rets)
{
    std::lock_guard<std::mutex> locker(m_mutex);
    return m_io_factory.read_input_ios(count_io, rets);
}

bool IoHardInterface::set_interval_reg(SetFrequency &reg)
{
    std::lock_guard<std::mutex> locker(m_mutex);

    std::string name = reg.name;
    int32_t index = reg.index;
    bool set_level = reg.level;

    bool frequency = reg.frequency;
    int32_t on_time = reg.on_time;
    int32_t off_time = reg.off_time;

    if (index < 0)
    {
        DEBUG_ERROR_OUT("set_interval_reg index < 0");
        return false;
    }

    if (!frequency)
    {
        // if last set frenquency ; delete iter from io_list
        auto iter = io_output_objects.find(name);
        bool exist = iter != io_output_objects.end();
        if (exist)
        {
            io_output_objects.erase(iter);
        }
        bool ret = set_output_io(index, set_level);
        DEBUG_OUT("set_interval_reg: index = %d, set_level = %d; set_output_io: ret = %d", index, set_level, (int)ret);
        return ret;
    }

    io_output_objects[name] = IntervalOutIo(m_priNh->get_clock(), index, on_time, off_time);
    io_output_objects[name].start();
    DEBUG_OUT("io_output_objects:size=%ld", io_output_objects.size());
    return true;
}

// 读取模拟量输入状态
// 暂时设置读写互斥
bool IoHardInterface::read_inputsA(const int &count, std::vector<BaseInterface::WORD> &rets)
{
    std::lock_guard<std::mutex> locker(m_readA_mutex);
    return m_io_factory.read_analog_inputs(count, rets);
}

// 设置单路模拟量输出
bool IoHardInterface::set_outputsA(const int &index, const BaseInterface::WORD &raw_data)
{
    std::lock_guard<std::mutex> locker(m_writeA_mutex);
    return m_io_factory.set_analog_output(index, raw_data);
}

// 读取参数服务器配置的io参数
bool IoHardInterface::read_param_server()
{
    // 读取不同模块内参数
    bool ret = read_module_param();

    IoGroup groupParam;
    m_priNh->get_parameter_or("num_digital_input", groupParam.num_input, 0);
    m_priNh->get_parameter_or("num_digital_output", groupParam.num_output, 0);
    m_priNh->get_parameter_or("num_analog_input", groupParam.num_analog_input, 0);
    m_priNh->get_parameter_or("num_analog_output", groupParam.num_analog_output, 0);

    // 调整io储存大小
    groupParam.resize();

    // 初始化工厂对象
    ret &= m_io_factory.init(groupParam);

    DEBUG_STREAM_OUT("IoHardInterface read_param_server ret=" << ret);
    return ret;
}

// 读取模块ros参数服务器字段
bool IoHardInterface::read_module_param()
{
    const UnitName unit;
    std::string prefix = "module";
    for (int i = 0; i < MAX_MODULE_COUNT; i++)
    {
        ModuleParam type;
        std::string curPrefix = prefix + std::to_string(i + 1) + "/";
        type.module_number = i;
        try {
            type.num_digital_input = m_priNh->get_parameter_or(curPrefix + unit.num_digital_input, 0);
            type.num_digital_output = m_priNh->get_parameter_or(curPrefix + unit.num_digital_output, 0);
            type.num_analog_input = m_priNh->get_parameter_or(curPrefix + unit.num_analog_input, 0);
            type.num_analog_output = m_priNh->get_parameter_or(curPrefix + unit.num_analog_output, 0);
            type.interface_type = m_priNh->get_parameter_or(curPrefix + unit.interface_type, std::string("canopen"));
            type.modbus_slave_name = m_priNh->get_parameter_or(curPrefix + unit.modbus_slave_name, std::string("slave0"));
            type.start_addr = m_priNh->get_parameter_or(curPrefix + unit.start_addr, 0);
            type.can_port = m_priNh->get_parameter_or(curPrefix + unit.can_port, std::string("can0"));
            type.node_id = m_priNh->get_parameter_or(curPrefix + unit.node_id, 0);
        } catch (const rclcpp::ParameterTypeException& e) {
            DEBUG_ERROR_OUT("Parameter type error:" << e.what());
            continue;
        }

        if (!type.valid())
        {
            DEBUG_WARN_OUT("normal end read_module_param");
            break;
        }

        bool creat = m_io_factory.factory_fun(type);
        type.print();
        if (!creat)
        {
            DEBUG_ERROR_OUT("io_factory creat failed");
            return false;
        }
    }

    return true;
}


// 带有频率动态输出IO-线程
void IoHardInterface::thread_fun(IoHardInterface *pThis)
{
    if (!pThis)
        return;

    // 50hz
    while (!pThis->m_quit_mutex.try_lock_for(std::chrono::milliseconds(20)))
    {
        pThis->cycle();
    }
}

void IoHardInterface::cycle()
{
    if (m_init_ok)
    {
        output_io_cycle();
    }
}

bool IoHardInterface::set_output_io(const int &index, const bool &level)
{
    return m_io_factory.set_output_io(index, level);
}

// 暂时设置读写互斥
//  int32 addr_index
//  bool  level
//  ---
//  bool  ret
void IoHardInterface::output_io_cycle()
{
    std::lock_guard<std::mutex> locker(m_mutex);
    for (auto iter = io_output_objects.begin(); iter != io_output_objects.end(); iter++)
    {
        IntervalOutIo::TargetLevel set_io;
        bool need_set_io = (iter->second).intervalOutput(set_io);
        if (need_set_io)
        {
            int addr = set_io.addr_index;
            bool level = set_io.level;
            set_output_io(addr, level);
        }
    }
}

void IoHardInterface::start_output()
{
    std::lock_guard<std::mutex> locker(m_mutex);
    for (auto iter = io_output_objects.begin(); iter != io_output_objects.end(); iter++)
    {
        (iter->second).start();
    }
}

void IoHardInterface::stop_output()
{
    std::lock_guard<std::mutex> locker(m_mutex);

    for (auto iter = io_output_objects.begin(); iter != io_output_objects.end(); iter++)
    {
        (iter->second).stop();
    }
}
}
